package cn.krund.lyc.hotlerobot.Ros.service.communication;


import android.util.Log;

import java.io.DataInputStream;
import java.io.IOException;

import cn.krund.lyc.hotlerobot.Ros.service.communicationinfo.RosProtocolInfo;
import cn.krund.lyc.hotlerobot.utils.DataConversionUtil;


/**
 * Created by yuzhantao on 16/10/8.
 * 网络协议预处理
 */
public class NetProtocolPretreatment {
    /**
     * byte转float      byte是基本类型
     *
     * @param bs    需要转换的byte数组
     * @param index 需要转换的起始位
     * @return
     */
    public int byte2Int(byte[] bs, int index) {
        int value = 0;

        for (int i = 0; i < 4; i++) {
            int shift = (4 - 1 - i) * 8;
            value += (bs[index + 3 - i] & 0x000000FF) << shift;//往高位游
        }
        return value;
    }


    public void getNetProtocol(byte[] data, int readLen, IRosCommunicationListener communicationListener) {
        try {
            RosProtocolInfo netProtocol = new RosProtocolInfo();
            byte[] bytes = new byte[readLen];
            System.arraycopy(data, 0, bytes, 0, readLen);
            String value = DataConversionUtil.bytes2HexString(bytes);
            Log.i("OSPF", "收到透传数据：" + value);
            for (int i = 0; i < readLen; i++) {
                if (bytes[i] == Integer.valueOf("AA", 16).byteValue()) {
                    netProtocol.setHead(new byte[]{bytes[i], bytes[i + 1]});
                    netProtocol.setType(bytes[i + 2]);
                    netProtocol.setCmd(bytes[i + 3]);
                    if (netProtocol.getCmd() == 0x07) {  // 地图数据
                        continue;
                    }
                    byte[] dataLen = new byte[]{bytes[i + 4], bytes[i + 5], bytes[i + 6], bytes[i + 7]};
                    int len = byte2Int(dataLen, 0);
                    byte[] readData = new byte[len - 1];
                    System.arraycopy(bytes, i + 8, readData, 0, len - 1);
                    netProtocol.setData(readData);
                    netProtocol.calcChkSum();
                    if (netProtocol.getChkSum() == bytes[i + 7 + len]) {
                        communicationListener.recv(netProtocol);
                        return;
                    }
                }
            }
            System.gc();
        } catch (Exception e) {
            e.printStackTrace();
        }
    }
}
